Sensor Fusion for Kinetis MCUs (ISSDK/KSDK version)
magnetic.c
Go to the documentation of this file.
1 // Copyright (c) 2014, 2015, 2016, NXP Semiconductors N.V.,
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of NXP Semiconductors N.V. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL NXP SEMICONDUCTORS N.V. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 
26 /*! \file magnetic.c
27  \brief Lower level magnetic calibration interface
28 
29  Many developers can utilize the NXP Sensor Fusion Library without ever
30  making any adjustment to the lower level magnetic calibration functions
31  defined in this file.
32 */
33 
34 #include "sensor_fusion.h"
35 #include "math.h"
36 #include "stdlib.h"
37 #include "time.h"
38 
39 #if F_USING_MAG
40 // function resets the magnetometer buffer and magnetic calibration
41 void fInitializeMagCalibration(MagCalibration *pthisMagCal,
42  MagBuffer *pthisMagBuffer)
43 {
44  float *pFlash; // pointer into flash memory
45  int8 i,
46  j; // loop counters
47 
48  // set magnetic buffer index to invalid value -1 to denote no measurement present
49  pthisMagBuffer->iMagBufferCount = 0;
50  for (i = 0; i < MAGBUFFSIZEX; i++)
51  for (j = 0; j < MAGBUFFSIZEY; j++) pthisMagBuffer->index[i][j] = -1;
52 
53  // initialize the array of (MAGBUFFSIZEX - 1) elements of 100 * tangents used for buffer indexing
54  // entries cover the range 100 * tan(-PI/2 + PI/MAGBUFFSIZEX), 100 * tan(-PI/2 + 2*PI/MAGBUFFSIZEX) to
55  // 100 * tan(-PI/2 + (MAGBUFFSIZEX - 1) * PI/MAGBUFFSIZEX).
56  // for MAGBUFFSIZEX=12, the entries range in value from -373 to +373
57  for (i = 0; i < (MAGBUFFSIZEX - 1); i++)
58  pthisMagBuffer->tanarray[i] = (int16) (100.0F * tanf(PI * (-0.5F + (float) (i + 1) / MAGBUFFSIZEX)));
59 
60  // check to see if the stored magnetic calibration has been erased
61  // the standard value for erased flash is 0xFF in each byte but for portability check against 0x12345678
62  pFlash = (float *) (CALIBRATION_NVM_ADDR + MAG_NVM_OFFSET);
63  if (*((uint32 *) pFlash++) == 0x12345678)
64  {
65  // a magnetic calibration is present in flash
66  // copy magnetic calibration elements (15x float + 1x int32 total 64 bytes) from flash to RAM
67  for (i = CHX; i <= CHZ; i++) pthisMagCal->fV[i] = *(pFlash++);
68  for (i = CHX; i <= CHZ; i++)
69  for (j = CHX; j <= CHZ; j++)
70  pthisMagCal->finvW[i][j] = *(pFlash++);
71  pthisMagCal->fB = *(pFlash++);
72  pthisMagCal->fBSq = *(pFlash++);
73  pthisMagCal->fFitErrorpc = *(pFlash++);
74  pthisMagCal->iValidMagCal = *((int32 *) pFlash);
75  }
76  else
77  {
78  // flash has been erased and no magnetic calibration is present
79  // initialize the magnetic calibration in RAM to null default
80  pthisMagCal->fV[CHX] = pthisMagCal->fV[CHY] = pthisMagCal->fV[CHZ] = 0.0F;
81  f3x3matrixAeqI(pthisMagCal->finvW);
82  pthisMagCal->fB = DEFAULTB;
83  pthisMagCal->fBSq = DEFAULTB * DEFAULTB;
84  pthisMagCal->fFitErrorpc = 100.0F;
85  pthisMagCal->iValidMagCal = 0;
86  }
87 
88  // initialize remaining elements of the magnetic calibration structure
89  pthisMagCal->iCalInProgress = 0;
90  pthisMagCal->iInitiateMagCal = 0;
91  pthisMagCal->iNewCalibrationAvailable = 0;
92  pthisMagCal->iMagBufferReadOnly = false;
93  pthisMagCal->i4ElementSolverTried = false;
94  pthisMagCal->i7ElementSolverTried = false;
95  pthisMagCal->i10ElementSolverTried = false;
96 
97  return;
98 }
99 
100 // function updates the magnetic measurement buffer with most recent magnetic data (typically 200Hz)
101 
102 // the uncalibrated measurements iBs are stored in the buffer but the calibrated measurements iBc are used for indexing.
103 void iUpdateMagBuffer(MagBuffer *pthisMagBuffer, MagSensor *pthisMag,
104  int32 loopcounter)
105 {
106  // local variables
107  int32 idelta; // absolute vector distance
108  int32 i; // counter
109  int16 itanj,
110  itank; // indexing accelerometer ratios
111  int8 j,
112  k,
113  l,
114  m; // counters
115  int8 itooclose; // flag denoting measurement is too close to existing ones
116 
117  // calculate the magnetometer buffer bins from the tangent ratios
118  if (pthisMag->iBc[CHZ] == 0) return;
119  itanj = (100 * (int32) pthisMag->iBc[CHX]) / ((int32) pthisMag->iBc[CHZ]);
120  itank = (100 * (int32) pthisMag->iBc[CHY]) / ((int32) pthisMag->iBc[CHZ]);
121 
122  // map tangent ratios to bins j and k using equal angle bins: C guarantees left to right execution of the test
123  // and add an offset of MAGBUFFSIZEX bins to k to mimic atan2 on this ratio
124  // j will vary from 0 to MAGBUFFSIZEX - 1 and k from 0 to 2 * MAGBUFFSIZEX - 1
125  j = k = 0;
126  while ((j < (MAGBUFFSIZEX - 1) && (itanj >= pthisMagBuffer->tanarray[j])))
127  j++;
128  while ((k < (MAGBUFFSIZEX - 1) && (itank >= pthisMagBuffer->tanarray[k])))
129  k++;
130  if (pthisMag->iBc[CHX] < 0) k += MAGBUFFSIZEX;
131 
132  // case 1: buffer is full and this bin has a measurement: over-write without increasing number of measurements
133  // this is the most common option at run time
134  if ((pthisMagBuffer->iMagBufferCount == MAXMEASUREMENTS) &&
135  (pthisMagBuffer->index[j][k] != -1))
136  {
137  // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
138  for (i = CHX; i <= CHZ; i++)
139  {
140  pthisMagBuffer->iBs[i][j][k] = pthisMag->iBs[i];
141  }
142 
143  pthisMagBuffer->index[j][k] = loopcounter;
144  return;
145  } // end case 1
146 
147  // case 2: the buffer is full and this bin does not have a measurement: store and retire the oldest
148  // this is the second most common option at run time
149  if ((pthisMagBuffer->iMagBufferCount == MAXMEASUREMENTS) &&
150  (pthisMagBuffer->index[j][k] == -1))
151  {
152  // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
153  for (i = CHX; i <= CHZ; i++)
154  {
155  pthisMagBuffer->iBs[i][j][k] = pthisMag->iBs[i];
156  }
157 
158  pthisMagBuffer->index[j][k] = loopcounter;
159 
160  // set l and m to the oldest active entry and disable it
161  i = loopcounter;
162  l = m = 0; // to avoid compiler complaint
163  for (j = 0; j < MAGBUFFSIZEX; j++)
164  {
165  for (k = 0; k < MAGBUFFSIZEY; k++)
166  {
167  // check if the time stamp is older than the oldest found so far (normally fails this test)
168  if (pthisMagBuffer->index[j][k] < i)
169  {
170  // check if this bin is active (normally passes this test)
171  if (pthisMagBuffer->index[j][k] != -1)
172  {
173  // set l and m to the indices of the oldest entry found so far
174  l = j;
175  m = k;
176 
177  // set i to the time stamp of the oldest entry found so far
178  i = pthisMagBuffer->index[l][m];
179  } // end of test for active
180  } // end of test for older
181  } // end of loop over k
182  } // end of loop over j
183 
184  // deactivate the oldest measurement (no need to zero the measurement data)
185  pthisMagBuffer->index[l][m] = -1;
186  return;
187  } // end case 2
188 
189  // case 3: buffer is not full and this bin is empty: store and increment number of measurements
190  if ((pthisMagBuffer->iMagBufferCount < MAXMEASUREMENTS) &&
191  (pthisMagBuffer->index[j][k] == -1))
192  {
193  // store the fast (unaveraged at typically 200Hz) integer magnetometer reading into the buffer bin j, k
194  for (i = CHX; i <= CHZ; i++)
195  {
196  pthisMagBuffer->iBs[i][j][k] = pthisMag->iBs[i];
197  }
198 
199  pthisMagBuffer->index[j][k] = loopcounter;
200  (pthisMagBuffer->iMagBufferCount)++;
201  return;
202  } // end case 3
203 
204  // case 4: buffer is not full and this bin has a measurement: over-write if close or try to slot in
205  // elsewhere if not close to the other measurements so as to create a mesh at power up
206  if ((pthisMagBuffer->iMagBufferCount < MAXMEASUREMENTS) &&
207  (pthisMagBuffer->index[j][k] != -1))
208  {
209  // calculate the vector difference between current measurement and the buffer entry
210  idelta = 0;
211  for (i = CHX; i <= CHZ; i++)
212  {
213  idelta += abs((int32) pthisMag->iBs[i] -
214  (int32) pthisMagBuffer->iBs[i][j][k]);
215  }
216 
217  // check to see if the current reading is close to this existing magnetic buffer entry
218  if (idelta < MESHDELTACOUNTS)
219  {
220  // simply over-write the measurement and return
221  for (i = CHX; i <= CHZ; i++)
222  {
223  pthisMagBuffer->iBs[i][j][k] = pthisMag->iBs[i];
224  }
225 
226  pthisMagBuffer->index[j][k] = loopcounter;
227  }
228  else
229  {
230  // reset the flag denoting that the current measurement is close to any measurement in the buffer
231  itooclose = 0;
232 
233  // to avoid compiler warning
234  l = m = 0;
235 
236  // loop over the buffer j from 0 potentially up to MAGBUFFSIZEX - 1
237  j = 0;
238  while (!itooclose && (j < MAGBUFFSIZEX))
239  {
240  // loop over the buffer k from 0 potentially up to MAGBUFFSIZEY - 1
241  k = 0;
242  while (!itooclose && (k < MAGBUFFSIZEY))
243  {
244  // check whether this buffer entry already has a measurement or not
245  if (pthisMagBuffer->index[j][k] != -1)
246  {
247  // calculate the vector difference between current measurement and the buffer entry
248  idelta = 0;
249  for (i = CHX; i <= CHZ; i++)
250  {
251  idelta += abs((int32) pthisMag->iBs[i] -
252  (int32) pthisMagBuffer->iBs[i][j][k]);
253  }
254 
255  // check to see if the current reading is close to this existing magnetic buffer entry
256  if (idelta < MESHDELTACOUNTS)
257  {
258  // set the flag to abort the search
259  itooclose = 1;
260  }
261  }
262  else
263  {
264  // store the location of this empty bin for future use
265  l = j;
266  m = k;
267  } // end of test for valid measurement in this bin
268 
269  k++;
270  } // end of k loop
271 
272  j++;
273  } // end of j loop
274 
275  // if none too close, store the measurement in the last empty bin found and return
276  // l and m are guaranteed to be set if no entries too close are detected
277  if (!itooclose)
278  {
279  for (i = CHX; i <= CHZ; i++)
280  {
281  pthisMagBuffer->iBs[i][l][m] = pthisMag->iBs[i];
282  }
283 
284  pthisMagBuffer->index[l][m] = loopcounter;
285  (pthisMagBuffer->iMagBufferCount)++;
286  }
287  } // end of test for closeness to current buffer entry
288 
289  return;
290  } // end case 4
291 
292  // this line should be unreachable
293  return;
294 }
295 
296 // function maps the uncalibrated magnetometer data fBs (uT) onto calibrated averaged data fBc (uT), iBc (counts)
297 void fInvertMagCal(MagSensor *pthisMag, MagCalibration *pthisMagCal)
298 {
299  // local variables
300  float ftmp[3]; // temporary array
301  int8 i; // loop counter
302 
303  // remove the computed hard iron offsets (uT): ftmp[]=fBs[]-V[]
304  for (i = CHX; i <= CHZ; i++)
305  {
306  ftmp[i] = pthisMag->fBs[i] - pthisMagCal->fV[i];
307  }
308 
309  // remove the computed soft iron offsets (uT and counts): fBc=inv(W)*(fBs[]-V[])
310  for (i = CHX; i <= CHZ; i++)
311  {
312  pthisMag->fBc[i] = pthisMagCal->finvW[i][CHX] *
313  ftmp[CHX] +
314  pthisMagCal->finvW[i][CHY] *
315  ftmp[CHY] +
316  pthisMagCal->finvW[i][CHZ] *
317  ftmp[CHZ];
318  pthisMag->iBc[i] = (int16) (pthisMag->fBc[i] * pthisMag->fCountsPeruT);
319  }
320 
321  return;
322 }
323 
324 // function runs the magnetic calibration
325 void fRunMagCalibration(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer,
326  MagSensor *pthisMag, int32 loopcounter)
327 {
328  int8 i,
329  j; // loop counters
330 
331  // determine whether to initiate a new magnetic calibration
332  if (!pthisMagCal->iCalInProgress)
333  {
334  // clear the flag
335  pthisMagCal->iInitiateMagCal = 0;
336 
337  // try one calibration attempt with the best model available given the number of measurements
338  if ((pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS10CAL) &&
339  (!pthisMagCal->i10ElementSolverTried))
340  {
341  pthisMagCal->i10ElementSolverTried = true;
342  pthisMagCal->iInitiateMagCal = 10;
343  }
344  else if ((pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS7CAL) &&
345  (!pthisMagCal->i7ElementSolverTried))
346  {
347  pthisMagCal->i7ElementSolverTried = true;
348  pthisMagCal->iInitiateMagCal = 7;
349  }
350  else if ((pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS4CAL) &&
351  (!pthisMagCal->i4ElementSolverTried))
352  {
353  pthisMagCal->i4ElementSolverTried = true;
354  pthisMagCal->iInitiateMagCal = 4;
355  }
356 
357  // otherwise start a calibration at regular interval defined by CAL_INTERVAL_SECS
358  else if (!pthisMagCal->iInitiateMagCal &&
359  !(loopcounter % (CAL_INTERVAL_SECS * FUSION_HZ)))
360  {
361  if (pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS10CAL)
362  {
363  pthisMagCal->i10ElementSolverTried = true;
364  pthisMagCal->iInitiateMagCal = 10;
365  }
366  else if (pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS7CAL)
367  {
368  pthisMagCal->i7ElementSolverTried = true;
369  pthisMagCal->iInitiateMagCal = 7;
370  }
371  else if (pthisMagBuffer->iMagBufferCount >= MINMEASUREMENTS4CAL)
372  {
373  pthisMagCal->i4ElementSolverTried = true;
374  pthisMagCal->iInitiateMagCal = 4;
375  }
376  }
377 
378  // store the selected calibration model (if any) to be run
379  pthisMagCal->iCalInProgress = pthisMagCal->iInitiateMagCal;
380  }
381 
382  // on entry each of the calibration functions resets iInitiateMagCal and on completion sets
383  // iCalInProgress=0 and iNewCalibrationAvailable=4,7,10 according to the solver used
384  switch (pthisMagCal->iCalInProgress)
385  {
386  case 0:
387  break;
388 
389  case 4:
390  fUpdateMagCalibration4Slice(pthisMagCal, pthisMagBuffer, pthisMag);
391  break;
392 
393  case 7:
394  fUpdateMagCalibration7Slice(pthisMagCal, pthisMagBuffer, pthisMag);
395  break;
396 
397  case 10:
398  fUpdateMagCalibration10Slice(pthisMagCal, pthisMagBuffer, pthisMag);
399  break;
400 
401  default:
402  break;
403  }
404 
405  // evaluate the new calibration to determine whether to accept it
406  if (pthisMagCal->iNewCalibrationAvailable)
407  {
408  // the geomagnetic field strength must be in range (earth is 22uT to 67uT) with reasonable fit error
409  if ((pthisMagCal->ftrB >= MINBFITUT) && (pthisMagCal->ftrB <= MAXBFITUT) &&
410  (pthisMagCal->ftrFitErrorpc <= 15.0F))
411  {
412  // the fit error must be improved or be from a more sophisticated solver but still 5 bars (under 3.5% fit error)
413  if ((pthisMagCal->ftrFitErrorpc <= pthisMagCal->fFitErrorpc) || ((
414  pthisMagCal->iNewCalibrationAvailable > pthisMagCal->
415  iValidMagCal) && (pthisMagCal->ftrFitErrorpc <= 3.5F)))
416  {
417  // accept the new calibration
418  pthisMagCal->iValidMagCal = pthisMagCal->iNewCalibrationAvailable;
419  pthisMagCal->fFitErrorpc = pthisMagCal->ftrFitErrorpc;
420  pthisMagCal->fB = pthisMagCal->ftrB;
421  pthisMagCal->fBSq = pthisMagCal->fB * pthisMagCal->fB;
422  for (i = CHX; i <= CHZ; i++)
423  {
424  pthisMagCal->fV[i] = pthisMagCal->ftrV[i];
425  for (j = CHX; j <= CHZ; j++)
426  pthisMagCal->finvW[i][j] = pthisMagCal->ftrinvW[i][j];
427  }
428  }
429  }
430  else
431  {
432  // the magnetic buffer is presumed corrupted so clear out all measurements and restart calibration attempts
433  pthisMagBuffer->iMagBufferCount = 0;
434  for (i = 0; i < MAGBUFFSIZEX; i++)
435  for (j = 0; j < MAGBUFFSIZEY; j++)
436  pthisMagBuffer->index[i][j] = -1;
437  pthisMagCal->i4ElementSolverTried = false;
438  pthisMagCal->i7ElementSolverTried = false;
439  pthisMagCal->i10ElementSolverTried = false;
440  } // end of test for new calibration within field strength and fit error limits
441 
442  // reset the new calibration flag
443  pthisMagCal->iNewCalibrationAvailable = 0;
444  } // end of test for new calibration available
445 
446  // age the existing fit error very slowly to avoid one good calibration locking out future updates.
447  // this prevents a calibration remaining for ever if a unit is never powered down
448  if (pthisMagCal->iValidMagCal)
449  pthisMagCal->fFitErrorpc += 1.0F / ((float) FUSION_HZ * FITERRORAGINGSECS);
450 
451  return;
452 }
453 
454 // 4 element calibration using 4x4 matrix inverse
455 void fUpdateMagCalibration4Slice(MagCalibration *pthisMagCal,
456  MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
457 {
458  // local variables
459  int8 i,
460  j,
461  k; // loop counters
462 
463  // working arrays for 4x4 matrix inversion
464  float *pfRows[4];
465  int8 iColInd[4];
466  int8 iRowInd[4];
467  int8 iPivot[4];
468 
469  // reset the time slice to zero if iInitiateMagCal is set and then clear iInitiateMagCal
470  if (pthisMagCal->iInitiateMagCal)
471  {
472  pthisMagCal->itimeslice = 0;
473  pthisMagCal->iInitiateMagCal = false;
474  pthisMagCal->iMagBufferReadOnly = true;
475  }
476 
477  // time slice 0: 18.8K ticks = 0.39ms for 300 measurements on KL25Z
478  // initialize matrices and compute average of measurements in magnetic buffer
479  if (pthisMagCal->itimeslice == 0)
480  {
481  int16 iM; // number of measurements in the buffer
482 
483  // zero on and above diagonal matrix X^T.X (in fmatA), vector X^T.Y (in fvecA), scalar Y^T.Y (in fYTY)
484  pthisMagCal->fYTY = 0.0F;
485  for (i = 0; i < 4; i++)
486  {
487  pthisMagCal->fvecA[i] = 0.0F;
488  for (j = i; j < 4; j++) pthisMagCal->fmatA[i][j] = 0.0F;
489  }
490 
491  // zero total number of measurements and measurement sums
492  iM = 0;
493  for (i = 0; i < 3; i++) pthisMagCal->iSumBs[i] = 0;
494 
495  // compute the sum of measurements in the magnetic buffer
496  for (i = 0; i < MAGBUFFSIZEX; i++)
497  {
498  for (j = 0; j < MAGBUFFSIZEY; j++)
499  {
500  if (pthisMagBuffer->index[i][j] != -1)
501  {
502  iM++;
503  for (k = 0; k < 3; k++)
504  pthisMagCal->iSumBs[k] += (int32) pthisMagBuffer->iBs[k][i][j];
505  }
506  }
507  }
508 
509  // compute the magnetic buffer measurement averages with rounding
510  for (i = 0; i < 3; i++)
511  {
512  if (pthisMagCal->iSumBs[i] >= 0)
513  pthisMagCal->iMeanBs[i] =
514  (
515  pthisMagCal->iSumBs[i] +
516  ((int32) iM >> 1)
517  ) /
518  (int32) iM;
519  else
520  pthisMagCal->iMeanBs[i] =
521  (
522  pthisMagCal->iSumBs[i] -
523  ((int32) iM >> 1)
524  ) /
525  (int32) iM;
526  }
527 
528  // for defensive programming, re-store the number of active measurements in the buffer
529  pthisMagBuffer->iMagBufferCount = iM;
530 
531  // increment the time slice
532  (pthisMagCal->itimeslice)++;
533  } // end of time slice 0
534 
535  // time slices 1 to MAGBUFFSIZEX: each up to 81K ticks = 1.7ms
536  // accumulate the matrices fmatA=X^T.X and fvecA=X^T.Y and Y^T.Y from the magnetic buffer
537  else if ((pthisMagCal->itimeslice >= 1) &&
538  (pthisMagCal->itimeslice <= MAGBUFFSIZEX))
539  {
540  float fBsZeroMeanSq; // squared magnetic measurement (counts^2)
541  int32 iBsZeroMean[3]; // zero mean magnetic buffer measurement (counts)
542 
543  // accumulate the measurement matrix elements XTX (in fmatA), XTY (in fvecA) and YTY on the zero mean measurements
544  i = pthisMagCal->itimeslice - 1;
545  for (j = 0; j < MAGBUFFSIZEY; j++)
546  {
547  if (pthisMagBuffer->index[i][j] != -1)
548  {
549  // compute zero mean measurements
550  for (k = 0; k < 3; k++)
551  iBsZeroMean[k] = (int32) pthisMagBuffer->iBs[k][i][j] - (int32) pthisMagCal->iMeanBs[k];
552 
553  // accumulate the non-zero elements of zero mean XTX (in fmatA)
554  pthisMagCal->fmatA[0][0] += (float) (iBsZeroMean[0] * iBsZeroMean[0]);
555  pthisMagCal->fmatA[0][1] += (float) (iBsZeroMean[0] * iBsZeroMean[1]);
556  pthisMagCal->fmatA[0][2] += (float) (iBsZeroMean[0] * iBsZeroMean[2]);
557  pthisMagCal->fmatA[1][1] += (float) (iBsZeroMean[1] * iBsZeroMean[1]);
558  pthisMagCal->fmatA[1][2] += (float) (iBsZeroMean[1] * iBsZeroMean[2]);
559  pthisMagCal->fmatA[2][2] += (float) (iBsZeroMean[2] * iBsZeroMean[2]);
560 
561  // accumulate XTY (in fvecA)
562  fBsZeroMeanSq = (float)
563  (
564  iBsZeroMean[CHX] *
565  iBsZeroMean[CHX] +
566  iBsZeroMean[CHY] *
567  iBsZeroMean[CHY] +
568  iBsZeroMean[CHZ] *
569  iBsZeroMean[CHZ]
570  );
571  for (k = 0; k < 3; k++)
572  pthisMagCal->fvecA[k] += (float) iBsZeroMean[k] * fBsZeroMeanSq;
573  pthisMagCal->fvecA[3] += fBsZeroMeanSq;
574 
575  // accumulate fYTY
576  pthisMagCal->fYTY += fBsZeroMeanSq * fBsZeroMeanSq;
577  }
578  }
579 
580  // increment the time slice
581  (pthisMagCal->itimeslice)++;
582  } // end of time slices 1 to MAGBUFFSIZEX
583 
584  // time slice MAGBUFFSIZEX+1: 18.3K ticks = 0.38ms on KL25Z (constant) (stored in systick[2])
585  // re-enable magnetic buffer for writing and invert fmatB = fmatA = X^T.X in situ
586  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX + 1))
587  {
588  int8 ierror; // matrix inversion error flag
589 
590  // set fmatA[3][3] = X^T.X[3][3] to number of measurements found
591  pthisMagCal->fmatA[3][3] = (float) pthisMagBuffer->iMagBufferCount;
592 
593  // enable the magnetic buffer for writing now that the matrices have been computed
594  pthisMagCal->iMagBufferReadOnly = false;
595 
596  // set fmatA and fmatB to above diagonal elements of fmatA
597  for (i = 0; i < 4; i++)
598  {
599  for (j = 0; j <= i; j++)
600  pthisMagCal->fmatB[i][j] = pthisMagCal->fmatB[j][i] = pthisMagCal->fmatA[i][j] = pthisMagCal->fmatA[j][i];
601  }
602 
603  // set fmatB = inv(fmatB) = inv(X^T.X)
604  for (i = 0; i < 4; i++) pfRows[i] = pthisMagCal->fmatB[i];
605  fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4, &ierror);
606 
607  // increment the time slice
608  (pthisMagCal->itimeslice)++;
609  } // // end of time slice MAGBUFFSIZEX+1
610 
611  // time slice MAGBUFFSIZEX+2: 17.2K ticks = 0.36ms on KL25Z (constant) (stored in systick[3])
612  // compute the solution vector and the calibration coefficients
613  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX + 2))
614  {
615  float fE; // error function = r^T.r
616  float ftmp; // scratch
617 
618  // the trial inverse soft iron matrix invW always equals the identity matrix for 4 element calibration
619  f3x3matrixAeqI(pthisMagCal->ftrinvW);
620 
621  // calculate solution vector fvecB = beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecA (counts)
622  for (i = 0; i < 4; i++)
623  {
624  pthisMagCal->fvecB[i] = pthisMagCal->fmatB[i][0] * pthisMagCal->fvecA[0];
625  for (j = 1; j < 4; j++)
626  pthisMagCal->fvecB[i] += pthisMagCal->fmatB[i][j] * pthisMagCal->fvecA[j];
627  }
628 
629  // compute the hard iron vector (uT) correction for zero mean data
630  ftmp = 0.5F * pthisMag->fuTPerCount;
631  for (i = CHX; i <= CHZ; i++)
632  pthisMagCal->ftrV[i] = ftmp * pthisMagCal->fvecB[i];
633 
634  // compute the geomagnetic field strength B (uT)
635  pthisMagCal->ftrB = pthisMagCal->fvecB[3] *
636  pthisMag->fuTPerCount *
637  pthisMag->fuTPerCount;
638  for (i = CHX; i <= CHZ; i++)
639  pthisMagCal->ftrB += pthisMagCal->ftrV[i] * pthisMagCal->ftrV[i];
640  pthisMagCal->ftrB = sqrtf(fabs(pthisMagCal->ftrB));
641 
642  // add in the previously subtracted magnetic buffer mean to get true hard iron offset (uT)
643  ftmp = pthisMag->fuTPerCount / (float) pthisMagBuffer->iMagBufferCount;
644  for (i = CHX; i <= CHZ; i++)
645  pthisMagCal->ftrV[i] += (float) pthisMagCal->iSumBs[i] * ftmp;
646 
647  // calculate E = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
648  // set E = beta^T.(X^T.Y) = fvecB^T.fvecA
649  fE = pthisMagCal->fvecB[0] * pthisMagCal->fvecA[0];
650  for (i = 1; i < 4; i++)
651  fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
652 
653  // set E = YTY - 2 * beta^T.(X^T.Y) = YTY - 2 * E;
654  fE = pthisMagCal->fYTY - 2.0F * fE;
655 
656  // set fvecA = (X^T.X).beta = fmatA.fvecB
657  for (i = 0; i < 4; i++)
658  {
659  pthisMagCal->fvecA[i] = pthisMagCal->fmatA[i][0] * pthisMagCal->fvecB[0];
660  for (j = 1; j < 4; j++)
661  pthisMagCal->fvecA[i] += pthisMagCal->fmatA[i][j] * pthisMagCal->fvecB[j];
662  }
663 
664  // add beta^T.(X^T.X).beta = fvecB^T * fvecA to give fit error E (un-normalized counts^4)
665  for (i = 0; i < 4; i++)
666  fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
667 
668  // normalize fit error to the number of measurements and take square root to get error in uT^2
669  fE = sqrtf(fabs(fE) / (float) pthisMagBuffer->iMagBufferCount) *
670  pthisMag->fuTPerCount *
671  pthisMag->fuTPerCount;
672 
673  // obtain dimensionless error by dividing square square of the geomagnetic field and convert to percent
674  pthisMagCal->ftrFitErrorpc = 50.0F * fE / (pthisMagCal->ftrB * pthisMagCal->ftrB);
675 
676  // reset the calibration in progress flag to allow writing to the magnetic buffer and flag
677  // that a new 4 element calibration is available
678  pthisMagCal->iCalInProgress = 0;
679  pthisMagCal->iNewCalibrationAvailable = 4;
680  } // end of time slice MAGBUFFSIZEX+2
681 
682  return;
683 }
684 
685 // 7 element calibration using direct eigen-decomposition
686 void fUpdateMagCalibration7Slice(MagCalibration *pthisMagCal,
687  MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
688 {
689  // local variables
690  float fresidue; // eigen-decomposition residual sum
691  float ftmp; // scratch variable
692  int8 i,
693  j,
694  k,
695  l; // loop counters
696 #define MATRIX_7_SIZE 7
697  // reset the time slice to zero if iInitiateMagCal is set and then clear iInitiateMagCal
698  if (pthisMagCal->iInitiateMagCal)
699  {
700  pthisMagCal->itimeslice = 0;
701  pthisMagCal->iInitiateMagCal = false;
702  pthisMagCal->iMagBufferReadOnly = true;
703  }
704 
705  // time slice 0: 18.1K KL25Z ticks for 300 measurements = 0.38ms on KL25Z (variable) stored in systick[0]
706  // zero measurement matrix and calculate the mean values in the magnetic buffer
707  if (pthisMagCal->itimeslice == 0)
708  {
709  int16 iM; // number of measurements in the magnetic buffer
710 
711  // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
712  for (i = 0; i < MATRIX_7_SIZE; i++)
713  for (j = i; j < MATRIX_7_SIZE; j++)
714  pthisMagCal->fmatA[i][j] = 0.0F;
715 
716  // compute the sum of measurements in the magnetic buffer
717  iM = 0;
718  for (i = 0; i < 3; i++) pthisMagCal->iSumBs[i] = 0;
719  for (i = 0; i < MAGBUFFSIZEX; i++)
720  {
721  for (j = 0; j < MAGBUFFSIZEY; j++)
722  {
723  if (pthisMagBuffer->index[i][j] != -1)
724  {
725  iM++;
726  for (k = 0; k < 3; k++)
727  pthisMagCal->iSumBs[k] += (int32) pthisMagBuffer->iBs[k][i][j];
728  }
729  }
730  }
731 
732  // compute the magnetic buffer measurement averages with nearest integer rounding
733  for (i = 0; i < 3; i++)
734  {
735  if (pthisMagCal->iSumBs[i] >= 0)
736  pthisMagCal->iMeanBs[i] =
737  (
738  pthisMagCal->iSumBs[i] +
739  ((int32) iM >> 1)
740  ) /
741  (int32) iM;
742  else
743  pthisMagCal->iMeanBs[i] =
744  (
745  pthisMagCal->iSumBs[i] -
746  ((int32) iM >> 1)
747  ) /
748  (int32) iM;
749  }
750 
751  // as defensive programming also ensure the number of measurements found is re-stored
752  pthisMagBuffer->iMagBufferCount = iM;
753 
754  // increment the time slice for the next iteration
755  (pthisMagCal->itimeslice)++;
756  } // end of time slice 0
757 
758  // time slices 1 to MAGBUFFSIZEX * MAGBUFFSIZEY: accumulate matrices: 8.6K KL25Z ticks = 0.18ms (with max stored in systick[1])
759  else if ((pthisMagCal->itimeslice >= 1) &&
760  (pthisMagCal->itimeslice <= MAGBUFFSIZEX * MAGBUFFSIZEY))
761  {
762  // accumulate the symmetric matrix fmatA on the zero mean measurements
763  i = (pthisMagCal->itimeslice - 1) / MAGBUFFSIZEY; // matrix row i ranges 0 to MAGBUFFSIZEX-1
764  j = (pthisMagCal->itimeslice - 1) % MAGBUFFSIZEY; // matrix column j ranges 0 to MAGBUFFSIZEY-1
765  if (pthisMagBuffer->index[i][j] != -1)
766  {
767  // set fvecA to be vector of zero mean measurements and their squares
768  for (k = 0; k < 3; k++)
769  {
770  pthisMagCal->fvecA[k + 3] = (float)
771  (
772  (int32) pthisMagBuffer->iBs[k][i][j] -
773  (int32) pthisMagCal->iMeanBs[k]
774  );
775  pthisMagCal->fvecA[k] = pthisMagCal->fvecA[k + 3] * pthisMagCal->fvecA[k + 3];
776  }
777 
778  // update non-zero elements fmatA[0-2][6] of fmatA ignoring fmatA[6][6] which is set later.
779  // elements fmatA[3-5][6] are zero as a result of subtracting the mean value
780  for (k = 0; k < 3; k++)
781  pthisMagCal->fmatA[k][6] += pthisMagCal->fvecA[k];
782 
783  // update the remaining on and above diagonal elements fmatA[0-5][0-5]
784  for (k = 0; k < (MATRIX_7_SIZE - 1); k++)
785  {
786  for (l = k; l < (MATRIX_7_SIZE - 1); l++)
787  pthisMagCal->fmatA[k][l] += pthisMagCal->fvecA[k] * pthisMagCal->fvecA[l];
788  }
789  }
790 
791  // increment the time slice for the next iteration
792  (pthisMagCal->itimeslice)++;
793  } // end of time slices 1 to MAGBUFFSIZEX * MAGBUFFSIZEY
794 
795  // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 1: 0.8K ticks on KL25Z = 0.02ms on KL25Z (constant) (stored in systick[2])
796  // re-enable magnetic buffer for writing and prepare fmatA, fmatB, fvecA for eigendecomposition
797  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 1))
798  {
799  // set fmatA[6][6] to the number of magnetic measurements found
800  pthisMagCal->fmatA[MATRIX_7_SIZE - 1][MATRIX_7_SIZE - 1] = (float) pthisMagBuffer->iMagBufferCount;
801 
802  // clear the magnetic buffer read only flag now that the matrices have been computed
803  pthisMagCal->iMagBufferReadOnly = false;
804 
805  // set below diagonal elements of 7x7 matrix fmatA to above diagonal elements
806  for (i = 1; i < MATRIX_7_SIZE; i++)
807  for (j = 0; j < i; j++)
808  pthisMagCal->fmatA[i][j] = pthisMagCal->fmatA[j][i];
809 
810  // set matrix of eigenvectors fmatB to identity matrix and eigenvalues vector fvecA to diagonal elements of fmatA
811  for (i = 0; i < MATRIX_7_SIZE; i++)
812  {
813  for (j = 0; j < MATRIX_7_SIZE; j++)
814  pthisMagCal->fmatB[i][j] = 0.0F;
815  pthisMagCal->fmatB[i][i] = 1.0F;
816  pthisMagCal->fvecA[i] = pthisMagCal->fmatA[i][i];
817  }
818 
819  // increment the time slice for the next iteration
820  (pthisMagCal->itimeslice)++;
821  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 1
822 
823  // repeating 21 time slices MAGBUFFSIZEX * MAGBUFFSIZEY + 2 to MAGBUFFSIZEX * MAGBUFFSIZEY + 22 inclusive
824  // to perform the eigendecomposition of the measurement matrix fmatA.
825  // 19.6k ticks = 0.41ms on KL25Z (with max stored in systick[3]).
826  // for a 7x7 matrix there are 21 above diagonal elements: 6+5+4+3+2+1+0=21
827  else if ((pthisMagCal->itimeslice >= (MAGBUFFSIZEX * MAGBUFFSIZEY + 2)) &&
828  (pthisMagCal->itimeslice <= (MAGBUFFSIZEX * MAGBUFFSIZEY + 22)))
829  {
830  // set k to the matrix element in range 0 to 20 to be zeroed and used it to set row i and column j
831  k = pthisMagCal->itimeslice - (MAGBUFFSIZEX * MAGBUFFSIZEY + 2);
832  if (k < 6)
833  {
834  i = 0;
835  j = k + 1;
836  }
837  else if (k < 11)
838  {
839  i = 1;
840  j = k - 4;
841  }
842  else if (k < 15)
843  {
844  i = 2;
845  j = k - 8;
846  }
847  else if (k < 18)
848  {
849  i = 3;
850  j = k - 11;
851  }
852  else if (k < 20)
853  {
854  i = 4;
855  j = k - 13;
856  }
857  else
858  {
859  i = 5;
860  j = 6;
861  }
862 
863  // only continue if matrix element i, j has not already been zeroed
864  if (fabsf(pthisMagCal->fmatA[i][j]) > 0.0F)
865  fComputeEigSlice(pthisMagCal->fmatA, pthisMagCal->fmatB,
866  pthisMagCal->fvecA, i, j, MATRIX_7_SIZE);
867 
868  // increment the time slice for the next iteration
869  (pthisMagCal->itimeslice)++;
870  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 2 to MAGBUFFSIZEX * MAGBUFFSIZEY + 22 inclusive
871 
872  // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 23: 2.6k ticks on KL25Z = 0.05ms on KL25Z (constant) (stored in systick[4])
873  // compute the sum of the absolute values of above diagonal elements in fmatA as eigen-decomposition exit criterion
874  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 23))
875  {
876  // sum residue of all above-diagonal elements
877  fresidue = 0.0F;
878  for (i = 0; i < MATRIX_7_SIZE; i++)
879  for (j = i + 1; j < MATRIX_7_SIZE; j++)
880  fresidue += fabsf(pthisMagCal->fmatA[i][j]);
881 
882  // determine whether to re-enter the eigen-decomposition or skip to calculation of the calibration coefficients
883  if (fresidue > 0.0F)
884  // continue the eigen-decomposition
885  (pthisMagCal->itimeslice) = MAGBUFFSIZEX * MAGBUFFSIZEY + 2;
886  else
887  // continue to compute the calibration coefficients since the eigen-decomposition is complete
888  (pthisMagCal->itimeslice)++;
889  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 23
890 
891  // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 24: 27.8k ticks = 0.58ms on KL25Z (constant) (stored in systick[5])
892  // compute the calibration coefficients from the solution eigenvector
893  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 24))
894  {
895  float fdetA; // determinant of ellipsoid matrix A
896  int8 imin; // column of solution eigenvector with minimum eigenvalue
897 
898  // set imin to the index of the smallest eigenvalue in fvecA
899  imin = 0;
900  for (i = 1; i < MATRIX_7_SIZE; i++)
901  if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[imin]) imin = i;
902 
903  // the ellipsoid fA must have positive determinant but the eigensolver can equally easily return a negated
904  // normalized eigenvector without changing the eigenvalue. compute the determinant of ellipsoid matrix A
905  // from first three elements of eigenvector and negate the eigenvector if negative.
906  fdetA = pthisMagCal->fmatB[CHX][imin] *
907  pthisMagCal->fmatB[CHY][imin] *
908  pthisMagCal->fmatB[CHZ][imin];
909  if (fdetA < 0.0F)
910  {
911  fdetA = fabs(fdetA);
912  for (i = 0; i < MATRIX_7_SIZE; i++)
913  pthisMagCal->fmatB[i][imin] = -pthisMagCal->fmatB[i][imin];
914  }
915 
916  // set diagonal elements of ellipsoid matrix A to the solution vector imin with smallest eigenvalue and
917  // zero off-diagonal elements since these are always zero in the 7 element model
918  // compute the hard iron offset fV for zero mean data (counts)
919  for (i = CHX; i <= CHZ; i++)
920  pthisMagCal->ftrV[i] = -0.5F *
921  pthisMagCal->fmatB[i + 3][imin] /
922  pthisMagCal->fmatB[i][imin];
923 
924  // set ftmp to the square of the geomagnetic field strength fB (counts) corresponding to current value of fdetA
925  ftmp = -pthisMagCal->fmatB[6][imin];
926  for (i = CHX; i <= CHZ; i++)
927  ftmp += pthisMagCal->fmatB[i][imin] *
928  pthisMagCal->ftrV[i] *
929  pthisMagCal->ftrV[i];
930 
931  // calculate the trial normalized fit error as a percentage normalized by the geomagnetic field strength squared
932  pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabsf(pthisMagCal->fvecA[imin]) /
933  (float) pthisMagBuffer->iMagBufferCount) / fabsf(ftmp);
934 
935  // compute the geomagnetic field strength (uT) for current value of fdetA
936  pthisMagCal->ftrB = sqrtf(fabsf(ftmp)) * pthisMag->fuTPerCount;
937 
938  // compute the normalized ellipsoid matrix A with unit determinant and derive invW also with unit determinant.
939  ftmp = powf(fabs(fdetA), -(ONETHIRD));
940  for (i = CHX; i <= CHZ; i++)
941  {
942  pthisMagCal->fA[i][i] = pthisMagCal->fmatB[i][imin] * ftmp;
943  pthisMagCal->ftrinvW[i][i] = sqrtf(fabsf(pthisMagCal->fA[i][i]));
944  }
945 
946  pthisMagCal->fA[CHX][CHY] = pthisMagCal->fA[CHX][CHZ] = pthisMagCal->fA[CHY][CHZ] = 0.0F;
947  pthisMagCal->ftrinvW[CHX][CHY] = pthisMagCal->ftrinvW[CHX][CHZ] = pthisMagCal->ftrinvW[CHY][CHZ] = 0.0F;
948 
949  // each element of fA has been scaled by fdetA^(-1/3) so the square of geomagnetic field strength B^2
950  // must be adjusted by the same amount and B by the square root to keep the ellipsoid equation valid:
951  // (Bk-V)^T.A.(Bk-V) = (Bk-V)^T.(invW)^T.invW.(Bk-V) = B^2
952  pthisMagCal->ftrB *= sqrt(fabs(ftmp));
953 
954  // compute the final hard iron offset (uT) by adding in previously subtracted zero mean offset (counts)
955  for (i = CHX; i <= CHZ; i++)
956  pthisMagCal->ftrV[i] =
957  (
958  pthisMagCal->ftrV[i] +
959  (float) pthisMagCal->iMeanBs[i]
960  ) *
961  pthisMag->fuTPerCount;
962 
963  // reset the calibration in progress flag to allow writing to the magnetic buffer and flag
964  // that a new 7 element calibration is available
965  pthisMagCal->iCalInProgress = 0;
966  pthisMagCal->iNewCalibrationAvailable = 7;
967  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 24
968 
969  return;
970 }
971 
972 // 10 element calibration using direct eigen-decomposition
973 void fUpdateMagCalibration10Slice(MagCalibration *pthisMagCal,
974  MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
975 {
976  // local variables
977  float fresidue; // eigen-decomposition residual sum
978  float ftmp; // scratch variable
979  int8 i,
980  j,
981  k,
982  l; // loop counters
983 #define MATRIX_10_SIZE 10
984  // reset the time slice to zero if iInitiateMagCal is set and then clear iInitiateMagCal
985  if (pthisMagCal->iInitiateMagCal)
986  {
987  pthisMagCal->itimeslice = 0;
988  pthisMagCal->iInitiateMagCal = false;
989  pthisMagCal->iMagBufferReadOnly = true;
990  }
991 
992  // time slice 0: 18.7k KL25Z ticks for 300 measurements = 0.39ms on KL25Z (variable) stored in systick[0]
993  // zero measurement matrix fmatA and calculate the mean values in the magnetic buffer
994  if (pthisMagCal->itimeslice == 0)
995  {
996  int16 iM; // number of measurements in the magnetic buffer
997 
998  // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
999  for (i = 0; i < MATRIX_10_SIZE; i++)
1000  for (j = i; j < MATRIX_10_SIZE; j++)
1001  pthisMagCal->fmatA[i][j] = 0.0F;
1002 
1003  // compute the sum of measurements in the magnetic buffer
1004  iM = 0;
1005  for (i = 0; i < 3; i++) pthisMagCal->iSumBs[i] = 0;
1006  for (i = 0; i < MAGBUFFSIZEX; i++)
1007  {
1008  for (j = 0; j < MAGBUFFSIZEY; j++)
1009  {
1010  if (pthisMagBuffer->index[i][j] != -1)
1011  {
1012  iM++;
1013  for (k = 0; k < 3; k++)
1014  pthisMagCal->iSumBs[k] += (int32) pthisMagBuffer->iBs[k][i][j];
1015  }
1016  }
1017  }
1018 
1019  // compute the magnetic buffer measurement averages with nearest integer rounding
1020  for (i = 0; i < 3; i++)
1021  {
1022  if (pthisMagCal->iSumBs[i] >= 0)
1023  pthisMagCal->iMeanBs[i] =
1024  (
1025  pthisMagCal->iSumBs[i] +
1026  ((int32) iM >> 1)
1027  ) /
1028  (int32) iM;
1029  else
1030  pthisMagCal->iMeanBs[i] =
1031  (
1032  pthisMagCal->iSumBs[i] -
1033  ((int32) iM >> 1)
1034  ) /
1035  (int32) iM;
1036  }
1037 
1038  // as defensive programming also ensure the number of measurements found is re-stored
1039  pthisMagBuffer->iMagBufferCount = iM;
1040 
1041  // increment the time slice for the next iteration
1042  (pthisMagCal->itimeslice)++;
1043  } // end of time slice 0
1044 
1045  // time slices 1 to MAGBUFFSIZEX * MAGBUFFSIZEY: accumulate matrices: 17.9k KL25Z ticks = 0.37ms for 300 measurements
1046  // (with max measured stored in systick[1])
1047  else if ((pthisMagCal->itimeslice >= 1) &&
1048  (pthisMagCal->itimeslice <= MAGBUFFSIZEX * MAGBUFFSIZEY))
1049  {
1050  // accumulate the symmetric matrix fmatA on the zero mean measurements
1051  i = (pthisMagCal->itimeslice - 1) / MAGBUFFSIZEY; // matrix row i ranges 0 to MAGBUFFSIZEX-1
1052  j = (pthisMagCal->itimeslice - 1) % MAGBUFFSIZEY; // matrix column j ranges 0 to MAGBUFFSIZEY-1
1053  if (pthisMagBuffer->index[i][j] != -1)
1054  {
1055  // set fvecA[6-8] to the zero mean measurements
1056  for (k = 0; k < 3; k++)
1057  pthisMagCal->fvecA[k + 6] = (float)
1058  (
1059  (int32) pthisMagBuffer->iBs[k][i][j] -
1060  (int32) pthisMagCal->iMeanBs[k]
1061  );
1062 
1063  // compute fvecA[0-5] from fvecA[6-8]
1064  pthisMagCal->fvecA[0] = pthisMagCal->fvecA[6] * pthisMagCal->fvecA[6];
1065  pthisMagCal->fvecA[1] = 2.0F *
1066  pthisMagCal->fvecA[6] *
1067  pthisMagCal->fvecA[7];
1068  pthisMagCal->fvecA[2] = 2.0F *
1069  pthisMagCal->fvecA[6] *
1070  pthisMagCal->fvecA[8];
1071  pthisMagCal->fvecA[3] = pthisMagCal->fvecA[7] * pthisMagCal->fvecA[7];
1072  pthisMagCal->fvecA[4] = 2.0F *
1073  pthisMagCal->fvecA[7] *
1074  pthisMagCal->fvecA[8];
1075  pthisMagCal->fvecA[5] = pthisMagCal->fvecA[8] * pthisMagCal->fvecA[8];
1076 
1077  // update non-zero elements fmatA[0-5][9] of fmatA ignoring fmatA[9][9] which is set later.
1078  // elements fmatA[6-8][9] are zero as a result of subtracting the mean value
1079  for (k = 0; k < 6; k++)
1080  pthisMagCal->fmatA[k][9] += pthisMagCal->fvecA[k];
1081 
1082  // update the remaining on and above diagonal elements fmatA[0-8][0-8]
1083  for (k = 0; k < (MATRIX_10_SIZE - 1); k++)
1084  {
1085  for (l = k; l < (MATRIX_10_SIZE - 1); l++)
1086  pthisMagCal->fmatA[k][l] += pthisMagCal->fvecA[k] * pthisMagCal->fvecA[l];
1087  }
1088  }
1089 
1090  // increment the time slice for the next iteration
1091  (pthisMagCal->itimeslice)++;
1092  } // end of time slices 1 to MAGBUFFSIZEX * MAGBUFFSIZEY
1093 
1094  // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 1: 1.2k ticks on KL25Z = 0.025ms on KL25Z (constant) (stored in systick[2])
1095  // re-enable magnetic buffer for writing and prepare fmatA, fmatB, fvecA for eigendecomposition
1096  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 1))
1097  {
1098  // set fmatA[9][9] to the number of magnetic measurements found
1099  pthisMagCal->fmatA[MATRIX_10_SIZE - 1][MATRIX_10_SIZE - 1] = (float) pthisMagBuffer->iMagBufferCount;
1100 
1101  // clear the magnetic buffer read only flag now that the matrices have been computed
1102  pthisMagCal->iMagBufferReadOnly = false;
1103 
1104  // set below diagonal elements of 10x10 matrix fmatA to above diagonal elements
1105  for (i = 1; i < MATRIX_10_SIZE; i++)
1106  for (j = 0; j < i; j++)
1107  pthisMagCal->fmatA[i][j] = pthisMagCal->fmatA[j][i];
1108 
1109  // set matrix of eigenvectors fmatB to identity matrix and eigenvalues vector fvecA to diagonal elements of fmatA
1110  for (i = 0; i < MATRIX_10_SIZE; i++)
1111  {
1112  for (j = 0; j < MATRIX_10_SIZE; j++)
1113  pthisMagCal->fmatB[i][j] = 0.0F;
1114  pthisMagCal->fmatB[i][i] = 1.0F;
1115  pthisMagCal->fvecA[i] = pthisMagCal->fmatA[i][i];
1116  }
1117 
1118  // increment the time slice for the next iteration
1119  (pthisMagCal->itimeslice)++;
1120  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 1
1121 
1122  // repeating 45 time slices MAGBUFFSIZEX * MAGBUFFSIZEY + 2 to MAGBUFFSIZEX * MAGBUFFSIZEY + 46 inclusive
1123  // to perform the eigendecomposition of the measurement matrix fmatA.
1124  // 28.2k ticks = 0.56ms on KL25Z (with max stored in systick[3]).
1125  // for a 10x10 matrix there are 45 above diagonal elements: 9+8+7+6+5+4+3+2+1+0=45
1126  else if ((pthisMagCal->itimeslice >= (MAGBUFFSIZEX * MAGBUFFSIZEY + 2)) &&
1127  (pthisMagCal->itimeslice <= (MAGBUFFSIZEX * MAGBUFFSIZEY + 46)))
1128  {
1129  // set k to the matrix element of interest in range 0 to 44 to be zeroed and set row i and column j
1130  k = pthisMagCal->itimeslice - (MAGBUFFSIZEX * MAGBUFFSIZEY + 2);
1131  if (k < 9)
1132  {
1133  i = 0;
1134  j = k + 1;
1135  }
1136  else if (k < 17)
1137  {
1138  i = 1;
1139  j = k - 7;
1140  }
1141  else if (k < 24)
1142  {
1143  i = 2;
1144  j = k - 14;
1145  }
1146  else if (k < 30)
1147  {
1148  i = 3;
1149  j = k - 20;
1150  }
1151  else if (k < 35)
1152  {
1153  i = 4;
1154  j = k - 25;
1155  }
1156  else if (k < 39)
1157  {
1158  i = 5;
1159  j = k - 29;
1160  }
1161  else if (k < 42)
1162  {
1163  i = 6;
1164  j = k - 32;
1165  }
1166  else if (k < 44)
1167  {
1168  i = 7;
1169  j = k - 34;
1170  }
1171  else
1172  {
1173  i = 8;
1174  j = 9;
1175  }
1176 
1177  // only continue if matrix element i, j has not already been zeroed
1178  if (fabsf(pthisMagCal->fmatA[i][j]) > 0.0F)
1179  fComputeEigSlice(pthisMagCal->fmatA, pthisMagCal->fmatB,
1180  pthisMagCal->fvecA, i, j, MATRIX_10_SIZE);
1181 
1182  // increment the time slice for the next iteration
1183  (pthisMagCal->itimeslice)++;
1184  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 2 to MAGBUFFSIZEX * MAGBUFFSIZEY + 46 inclusive
1185 
1186  // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 47: 5.6k ticks on KL25Z = 0.12ms on KL25Z (constant) (stored in systick[4])
1187  // compute the sum of the absolute values of above diagonal elements in fmatA as eigen-decomposition exit criterion
1188  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 47))
1189  {
1190  // sum residue of all above-diagonal elements
1191  fresidue = 0.0F;
1192  for (i = 0; i < MATRIX_10_SIZE; i++)
1193  for (j = i + 1; j < MATRIX_10_SIZE; j++)
1194  fresidue += fabsf(pthisMagCal->fmatA[i][j]);
1195 
1196  // determine whether to re-enter the eigen-decomposition or skip to calculation of the calibration coefficients
1197  if (fresidue > 0.0F)
1198  // continue the eigen-decomposition
1199  (pthisMagCal->itimeslice) = MAGBUFFSIZEX * MAGBUFFSIZEY + 2;
1200  else
1201  // continue to compute the calibration coefficients since the eigen-decomposition is complete
1202  (pthisMagCal->itimeslice)++;
1203  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 47
1204 
1205  // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 48: 38.5k ticks = 0.80ms on KL25Z (constant) (stored in systick[5])
1206  // compute the calibration coefficients (excluding invW) from the solution eigenvector
1207  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 48))
1208  {
1209  float fdetA; // determinant of ellipsoid matrix A
1210  int8 imin; // column of solution eigenvector with minimum eigenvalue
1211 
1212  // set imin to the index of the smallest eigenvalue in fvecA
1213  imin = 0;
1214  for (i = 1; i < MATRIX_10_SIZE; i++)
1215  if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[imin]) imin = i;
1216 
1217  // set the ellipsoid matrix A from elements 0 to 5 of the solution eigenvector.
1218  pthisMagCal->fA[CHX][CHX] = pthisMagCal->fmatB[0][imin];
1219  pthisMagCal->fA[CHX][CHY] = pthisMagCal->fA[CHY][CHX] = pthisMagCal->fmatB[1][imin];
1220  pthisMagCal->fA[CHX][CHZ] = pthisMagCal->fA[CHZ][CHX] = pthisMagCal->fmatB[2][imin];
1221  pthisMagCal->fA[CHY][CHY] = pthisMagCal->fmatB[3][imin];
1222  pthisMagCal->fA[CHY][CHZ] = pthisMagCal->fA[CHZ][CHY] = pthisMagCal->fmatB[4][imin];
1223  pthisMagCal->fA[CHZ][CHZ] = pthisMagCal->fmatB[5][imin];
1224 
1225  // negate A and the entire solution vector A has negative determinant
1226  fdetA = f3x3matrixDetA(pthisMagCal->fA);
1227  if (fdetA < 0.0F)
1228  {
1229  f3x3matrixAeqMinusA(pthisMagCal->fA);
1230  fdetA = fabs(fdetA);
1231  for (i = 0; i < MATRIX_10_SIZE; i++)
1232  pthisMagCal->fmatB[i][imin] = -pthisMagCal->fmatB[i][imin];
1233  }
1234 
1235  // set finvA to the inverse of the ellipsoid matrix fA
1236  f3x3matrixAeqInvSymB(pthisMagCal->finvA, pthisMagCal->fA);
1237 
1238  // compute the hard iron offset fV for zero mean data (counts)
1239  for (i = CHX; i <= CHZ; i++)
1240  {
1241  pthisMagCal->ftrV[i] = pthisMagCal->finvA[i][0] *
1242  pthisMagCal->fmatB[6][imin] +
1243  pthisMagCal->finvA[i][1] *
1244  pthisMagCal->fmatB[7][imin] +
1245  pthisMagCal->finvA[i][2] *
1246  pthisMagCal->fmatB[8][imin];
1247  pthisMagCal->ftrV[i] *= -0.5F;
1248  }
1249 
1250  // compute the geomagnetic field strength B (counts) for current (un-normalized) ellipsoid matrix determinant.
1251  ftmp = pthisMagCal->fA[CHX][CHY] *
1252  pthisMagCal->ftrV[CHX] *
1253  pthisMagCal->ftrV[CHY] +
1254  pthisMagCal->fA[CHX][CHZ] *
1255  pthisMagCal->ftrV[CHX] *
1256  pthisMagCal->ftrV[CHZ] +
1257  pthisMagCal->fA[CHY][CHZ] *
1258  pthisMagCal->ftrV[CHY] *
1259  pthisMagCal->ftrV[CHZ];
1260  ftmp *= 2.0F;
1261  ftmp -= pthisMagCal->fmatB[9][imin];
1262  ftmp += pthisMagCal->fA[CHX][CHX] *
1263  pthisMagCal->ftrV[CHX] *
1264  pthisMagCal->ftrV[CHX];
1265  ftmp += pthisMagCal->fA[CHY][CHY] *
1266  pthisMagCal->ftrV[CHY] *
1267  pthisMagCal->ftrV[CHY];
1268  ftmp += pthisMagCal->fA[CHZ][CHZ] *
1269  pthisMagCal->ftrV[CHZ] *
1270  pthisMagCal->ftrV[CHZ];
1271  pthisMagCal->ftrB = sqrtf(fabsf(ftmp));
1272 
1273  // calculate the normalized fit error as a percentage
1274  pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabsf(
1275  pthisMagCal->fvecA[imin] /
1276  (float) pthisMagBuffer->iMagBufferCount
1277  )) / (pthisMagCal->ftrB * pthisMagCal->ftrB);
1278 
1279  // convert the trial geomagnetic field strength B from counts to uT for un-normalized soft iron matrix A
1280  pthisMagCal->ftrB *= pthisMag->fuTPerCount;
1281 
1282  // compute the final hard iron offset (uT) by adding in previously subtracted zero mean offset (counts)
1283  for (i = CHX; i <= CHZ; i++)
1284  pthisMagCal->ftrV[i] =
1285  (
1286  pthisMagCal->ftrV[i] +
1287  (float) pthisMagCal->iMeanBs[i]
1288  ) *
1289  pthisMag->fuTPerCount;
1290 
1291  // normalize the ellipsoid matrix A to unit determinant
1292  ftmp = powf(fabs(fdetA), -(ONETHIRD));
1293  f3x3matrixAeqAxScalar(pthisMagCal->fA, ftmp);
1294 
1295  // each element of fA has been scaled by fdetA^(-1/3) so the square of geomagnetic field strength B^2
1296  // must be adjusted by the same amount and B by the square root to keep the ellipsoid equation valid:
1297  // (Bk-V)^T.A.(Bk-V) = (Bk-V)^T.(invW)^T.invW.(Bk-V) = B^2
1298  pthisMagCal->ftrB *= sqrt(fabs(ftmp));
1299 
1300  // prepare for eigendecomposition of fA to compute finvW from the square root of fA by setting
1301  // fmatA (upper left) to the 3x3 matrix fA, set fmatB (eigenvectors to identity matrix and
1302  // fvecA (eigenvalues) to diagonal elements of fmatA = fA
1303  for (i = 0; i < 3; i++)
1304  {
1305  for (j = 0; j < 3; j++)
1306  {
1307  pthisMagCal->fmatA[i][j] = pthisMagCal->fA[i][j];
1308  pthisMagCal->fmatB[i][j] = 0.0F;
1309  }
1310 
1311  pthisMagCal->fmatB[i][i] = 1.0F;
1312  pthisMagCal->fvecA[i] = pthisMagCal->fmatA[i][i];
1313  }
1314 
1315  // increment the time slice for the next iteration
1316  (pthisMagCal->itimeslice)++;
1317  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 48
1318 
1319  // repeating 3 time slices MAGBUFFSIZEX * MAGBUFFSIZEY + 49 to MAGBUFFSIZEX * MAGBUFFSIZEY + 51 inclusive
1320  // 9.7kticks = 0.2ms on KL25Z (with max stored in systick[6]).
1321  // perform the eigendecomposition of the 3x3 ellipsoid matrix fA which has 3 above diagonal elements: 2+1+0=3
1322  else if ((pthisMagCal->itimeslice >= (MAGBUFFSIZEX * MAGBUFFSIZEY + 49)) &&
1323  (pthisMagCal->itimeslice <= (MAGBUFFSIZEX * MAGBUFFSIZEY + 51)))
1324  {
1325  // set k to the matrix element of interest in range 0 to 2 to be zeroed and set row i and column j
1326  k = pthisMagCal->itimeslice - (MAGBUFFSIZEX * MAGBUFFSIZEY + 49);
1327  if (k == 0)
1328  {
1329  i = 0;
1330  j = 1;
1331  }
1332  else if (k == 1)
1333  {
1334  i = 0;
1335  j = 2;
1336  }
1337  else
1338  {
1339  i = 1;
1340  j = 2;
1341  }
1342 
1343  // only continue with eigen-decomposition if matrix element i, j has not already been zeroed
1344  if (fabsf(pthisMagCal->fmatA[i][j]) > 0.0F)
1345  fComputeEigSlice(pthisMagCal->fmatA, pthisMagCal->fmatB,
1346  pthisMagCal->fvecA, i, j, 3);
1347 
1348  // increment the time slice for the next iteration
1349  (pthisMagCal->itimeslice)++;
1350  } // end of time slices MAGBUFFSIZEX * MAGBUFFSIZEY + 49 to MAGBUFFSIZEX * MAGBUFFSIZEY + 51 inclusive
1351 
1352  // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 52: 0.3k ticks = 0.006ms on KL25Z (constant) (stored in systick[7])
1353  // determine whether the eigen-decomposition of fA is complete
1354  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 52))
1355  {
1356  // sum residue of all above-diagonal elements and use to determine whether
1357  // to re-enter the eigen-decomposition or skip to calculation of the calibration coefficients
1358  fresidue = fabsf(pthisMagCal->fmatA[0][1]) + fabsf(pthisMagCal->fmatA[0][2]) + fabsf(pthisMagCal->fmatA[1][2]);
1359  if (fresidue > 0.0F)
1360  // continue the eigen-decomposition
1361  (pthisMagCal->itimeslice) = MAGBUFFSIZEX * MAGBUFFSIZEY + 49;
1362  else
1363  // continue to compute the calibration coefficients since the eigen-decomposition is complete
1364  (pthisMagCal->itimeslice)++;
1365  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 52
1366 
1367  // time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 53: 9.3k ticks = 0.19ms on KL25Z (constant) (stored in systick[8])
1368  // compute the inverse gain matrix invW from the eigendecomposition of the ellipsoid matrix A
1369  else if (pthisMagCal->itimeslice == (MAGBUFFSIZEX * MAGBUFFSIZEY + 53))
1370  {
1371  // set pthisMagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
1372  for (j = 0; j < 3; j++) // loop over columns j
1373  {
1374  ftmp = sqrtf(sqrtf(fabsf(pthisMagCal->fvecA[j])));
1375  for (i = 0; i < 3; i++) // loop over rows i
1376  pthisMagCal->fmatB[i][j] *= ftmp;
1377  }
1378 
1379  // set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
1380  // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
1381  // loop over on and above diagonal elements
1382  for (i = 0; i < 3; i++)
1383  {
1384  for (j = i; j < 3; j++)
1385  {
1386  pthisMagCal->ftrinvW[i][j] = pthisMagCal->ftrinvW[j][i] =
1387  pthisMagCal->fmatB[i][0] *
1388  pthisMagCal->fmatB[j][0] +
1389  pthisMagCal->fmatB[i][1] *
1390  pthisMagCal->fmatB[j][1] +
1391  pthisMagCal->fmatB[i][2] *
1392  pthisMagCal->fmatB[j][2];
1393  }
1394  }
1395 
1396  // reset the calibration in progress flag to allow writing to the magnetic buffer and flag
1397  // that a new 10 element calibration is available
1398  pthisMagCal->iCalInProgress = 0;
1399  pthisMagCal->iNewCalibrationAvailable = 10;
1400  } // end of time slice MAGBUFFSIZEX * MAGBUFFSIZEY + 53
1401 
1402  return;
1403 }
1404 
1405 // 4 element calibration using 4x4 matrix inverse
1406 void fComputeMagCalibration4(MagCalibration *pthisMagCal,
1407  MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
1408 {
1409  // local variables
1410  float fBs2; // fBs[CHX]^2+fBs[CHY]^2+fBs[CHZ]^2
1411  float fSumBs4; // sum of fBs2
1412  float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
1413  float fE; // error function = r^T.r
1414  int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
1415  int16 iCount; // number of measurements counted
1416  int8 ierror; // matrix inversion error flag
1417  int8 i,
1418  j,
1419  k,
1420  l; // loop counters
1421 
1422  // working arrays for 4x4 matrix inversion
1423  float *pfRows[4];
1424  int8 iColInd[4];
1425  int8 iRowInd[4];
1426  int8 iPivot[4];
1427 
1428  // compute fscaling to reduce multiplications later
1429  fscaling = pthisMag->fuTPerCount / DEFAULTB;
1430 
1431  // the trial inverse soft iron matrix invW always equals the identity matrix for 4 element calibration
1432  f3x3matrixAeqI(pthisMagCal->ftrinvW);
1433 
1434  // zero fSumBs4=Y^T.Y, fvecB=X^T.Y (4x1) and on and above diagonal elements of fmatA=X^T*X (4x4)
1435  fSumBs4 = 0.0F;
1436  for (i = 0; i < 4; i++)
1437  {
1438  pthisMagCal->fvecB[i] = 0.0F;
1439  for (j = i; j < 4; j++)
1440  {
1441  pthisMagCal->fmatA[i][j] = 0.0F;
1442  }
1443  }
1444 
1445  // the offsets are guaranteed to be set from the first element but to avoid compiler error
1446  iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0;
1447 
1448  // use entries from magnetic buffer to compute matrices
1449  iCount = 0;
1450  for (j = 0; j < MAGBUFFSIZEX; j++)
1451  {
1452  for (k = 0; k < MAGBUFFSIZEY; k++)
1453  {
1454  if (pthisMagBuffer->index[j][k] != -1)
1455  {
1456  // use first valid magnetic buffer entry as estimate (in counts) for offset
1457  if (iCount == 0)
1458  {
1459  for (l = CHX; l <= CHZ; l++)
1460  {
1461  iOffset[l] = pthisMagBuffer->iBs[l][j][k];
1462  }
1463  }
1464 
1465  // store scaled and offset fBs[XYZ] in fvecA[0-2] and fBs[XYZ]^2 in fvecA[3-5]
1466  for (l = CHX; l <= CHZ; l++)
1467  {
1468  pthisMagCal->fvecA[l] = (float)
1469  (
1470  (int32) pthisMagBuffer->iBs[l][j][k] -
1471  (int32) iOffset[l]
1472  ) * fscaling;
1473  pthisMagCal->fvecA[l + 3] = pthisMagCal->fvecA[l] * pthisMagCal->fvecA[l];
1474  }
1475 
1476  // calculate fBs2 = fBs[CHX]^2 + fBs[CHY]^2 + fBs[CHZ]^2 (scaled uT^2)
1477  fBs2 = pthisMagCal->fvecA[3] +
1478  pthisMagCal->fvecA[4] +
1479  pthisMagCal->fvecA[5];
1480 
1481  // accumulate fBs^4 over all measurements into fSumBs4=Y^T.Y
1482  fSumBs4 += fBs2 * fBs2;
1483 
1484  // now we have fBs2, accumulate fvecB[0-2] = X^T.Y =sum(fBs2.fBs[XYZ])
1485  for (l = CHX; l <= CHZ; l++)
1486  {
1487  pthisMagCal->fvecB[l] += pthisMagCal->fvecA[l] * fBs2;
1488  }
1489 
1490  //accumulate fvecB[3] = X^T.Y =sum(fBs2)
1491  pthisMagCal->fvecB[3] += fBs2;
1492 
1493  // accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
1494  pthisMagCal->fmatA[0][0] += pthisMagCal->fvecA[CHX + 3];
1495  pthisMagCal->fmatA[0][1] += pthisMagCal->fvecA[CHX] * pthisMagCal->fvecA[CHY];
1496  pthisMagCal->fmatA[0][2] += pthisMagCal->fvecA[CHX] * pthisMagCal->fvecA[CHZ];
1497  pthisMagCal->fmatA[0][3] += pthisMagCal->fvecA[CHX];
1498  pthisMagCal->fmatA[1][1] += pthisMagCal->fvecA[CHY + 3];
1499  pthisMagCal->fmatA[1][2] += pthisMagCal->fvecA[CHY] * pthisMagCal->fvecA[CHZ];
1500  pthisMagCal->fmatA[1][3] += pthisMagCal->fvecA[CHY];
1501  pthisMagCal->fmatA[2][2] += pthisMagCal->fvecA[CHZ + 3];
1502  pthisMagCal->fmatA[2][3] += pthisMagCal->fvecA[CHZ];
1503 
1504  // increment the counter for next iteration
1505  iCount++;
1506  }
1507  }
1508  }
1509 
1510  // set the last element of the measurement matrix to the number of buffer elements used
1511  pthisMagCal->fmatA[3][3] = (float) iCount;
1512 
1513  // store the number of measurements accumulated (defensive programming, should never be needed)
1514  pthisMagBuffer->iMagBufferCount = iCount;
1515 
1516  // use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X
1517  for (i = 0; i < 4; i++)
1518  {
1519  for (j = i; j < 4; j++)
1520  {
1521  pthisMagCal->fmatB[i][j] = pthisMagCal->fmatB[j][i] = pthisMagCal->fmatA[j][i] = pthisMagCal->fmatA[i][j];
1522  }
1523  }
1524 
1525  // calculate in situ inverse of fmatB = inv(X^T.X) (4x4) while fmatA still holds X^T.X
1526  for (i = 0; i < 4; i++)
1527  {
1528  pfRows[i] = pthisMagCal->fmatB[i];
1529  }
1530 
1531  fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4, &ierror);
1532 
1533  // calculate fvecA = solution beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecB
1534  for (i = 0; i < 4; i++)
1535  {
1536  pthisMagCal->fvecA[i] = 0.0F;
1537  for (k = 0; k < 4; k++)
1538  {
1539  pthisMagCal->fvecA[i] += pthisMagCal->fmatB[i][k] * pthisMagCal->fvecB[k];
1540  }
1541  }
1542 
1543  // calculate E = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
1544  // = fSumBs4 - 2 * fvecA^T.fvecB + fvecA^T.fmatA.fvecA
1545  // first set E = Y^T.Y - 2 * beta^T.(X^T.Y) = fSumBs4 - 2 * fvecA^T.fvecB
1546  fE = 0.0F;
1547  for (i = 0; i < 4; i++)
1548  {
1549  fE += pthisMagCal->fvecA[i] * pthisMagCal->fvecB[i];
1550  }
1551 
1552  fE = fSumBs4 - 2.0F * fE;
1553 
1554  // set fvecB = (X^T.X).beta = fmatA.fvecA
1555  for (i = 0; i < 4; i++)
1556  {
1557  pthisMagCal->fvecB[i] = 0.0F;
1558  for (k = 0; k < 4; k++)
1559  {
1560  pthisMagCal->fvecB[i] += pthisMagCal->fmatA[i][k] * pthisMagCal->fvecA[k];
1561  }
1562  }
1563 
1564  // complete calculation of E by adding beta^T.(X^T.X).beta = fvecA^T * fvecB
1565  for (i = 0; i < 4; i++)
1566  {
1567  fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
1568  }
1569 
1570  // compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
1571  for (l = CHX; l <= CHZ; l++)
1572  {
1573  pthisMagCal->ftrV[l] = 0.5F * pthisMagCal->fvecA[l];
1574  }
1575 
1576  // compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
1577  pthisMagCal->ftrB = sqrtf(pthisMagCal->fvecA[3] + pthisMagCal->ftrV[CHX] *
1578  pthisMagCal->ftrV[CHX] + pthisMagCal->ftrV[CHY] *
1579  pthisMagCal->ftrV[CHY] + pthisMagCal->ftrV[CHZ] *
1580  pthisMagCal->ftrV[CHZ]);
1581 
1582  // calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength
1583  pthisMagCal->ftrFitErrorpc = sqrtf(fE / (float) pthisMagBuffer->iMagBufferCount) * 100.0F / (2.0F * pthisMagCal->ftrB * pthisMagCal->ftrB);
1584 
1585  // correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
1586  for (l = CHX; l <= CHZ; l++)
1587  {
1588  pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] *
1589  DEFAULTB +
1590  (float) iOffset[l] *
1591  pthisMag->fuTPerCount;
1592  }
1593 
1594  // correct the geomagnetic field strength B to correct scaling (result in uT)
1595  pthisMagCal->ftrB *= DEFAULTB;
1596 
1597  return;
1598 }
1599 
1600 // 7 element calibration using direct eigen-decomposition
1601 void fComputeMagCalibration7(MagCalibration *pthisMagCal,
1602  MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
1603 {
1604  // local variables
1605  float det; // matrix determinant
1606  float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
1607  float ftmp; // scratch variable
1608  int16 iOffset[3]; // offset to remove large DC hard iron bias
1609  int16 iCount; // number of measurements counted
1610  int8 i,
1611  j,
1612  k,
1613  l,
1614  m,
1615  n; // loop counters
1616 
1617  // compute fscaling to reduce multiplications later
1618  fscaling = pthisMag->fuTPerCount / DEFAULTB;
1619 
1620  // the offsets are guaranteed to be set from the first element but to avoid compiler error
1621  iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0;
1622 
1623  // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
1624  for (m = 0; m < 7; m++)
1625  {
1626  for (n = m; n < 7; n++)
1627  {
1628  pthisMagCal->fmatA[m][n] = 0.0F;
1629  }
1630  }
1631 
1632  // add megnetic buffer entries into product matrix fmatA
1633  iCount = 0;
1634  for (j = 0; j < MAGBUFFSIZEX; j++)
1635  {
1636  for (k = 0; k < MAGBUFFSIZEY; k++)
1637  {
1638  if (pthisMagBuffer->index[j][k] != -1)
1639  {
1640  // use first valid magnetic buffer entry as offset estimate (bit counts)
1641  if (iCount == 0)
1642  {
1643  for (l = CHX; l <= CHZ; l++)
1644  {
1645  iOffset[l] = pthisMagBuffer->iBs[l][j][k];
1646  }
1647  }
1648 
1649  // apply the offset and scaling and store in fvecA
1650  for (l = CHX; l <= CHZ; l++)
1651  {
1652  pthisMagCal->fvecA[l + 3] = (float)
1653  (
1654  (int32) pthisMagBuffer->iBs[l][j][k] -
1655  (int32) iOffset[l]
1656  ) * fscaling;
1657  pthisMagCal->fvecA[l] = pthisMagCal->fvecA[l + 3] * pthisMagCal->fvecA[l + 3];
1658  }
1659 
1660  // accumulate the on-and above-diagonal terms of pthisMagCal->fmatA=Sigma{fvecA^T * fvecA}
1661  // with the exception of fmatA[6][6] which will sum to the number of measurements
1662  // and remembering that fvecA[6] equals 1.0F
1663  // update the right hand column [6] of fmatA except for fmatA[6][6]
1664  for (m = 0; m < 6; m++)
1665  {
1666  pthisMagCal->fmatA[m][6] += pthisMagCal->fvecA[m];
1667  }
1668 
1669  // update the on and above diagonal terms except for right hand column 6
1670  for (m = 0; m < 6; m++)
1671  {
1672  for (n = m; n < 6; n++)
1673  {
1674  pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
1675  }
1676  }
1677 
1678  // increment the measurement counter for the next iteration
1679  iCount++;
1680  }
1681  }
1682  }
1683 
1684  // finally set the last element fmatA[6][6] to the number of measurements
1685  pthisMagCal->fmatA[6][6] = (float) iCount;
1686 
1687  // store the number of measurements accumulated (defensive programming, should never be needed)
1688  pthisMagBuffer->iMagBufferCount = iCount;
1689 
1690  // copy the above diagonal elements of fmatA to below the diagonal
1691  for (m = 1; m < 7; m++)
1692  {
1693  for (n = 0; n < m; n++)
1694  {
1695  pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
1696  }
1697  }
1698 
1699  // set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
1700  fEigenCompute10(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB,
1701  7);
1702 
1703  // find the smallest eigenvalue
1704  j = 0;
1705  for (i = 1; i < 7; i++)
1706  {
1707  if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
1708  {
1709  j = i;
1710  }
1711  }
1712 
1713  // set ellipsoid matrix A to the solution vector with smallest eigenvalue, compute its determinant
1714  // and the hard iron offset (scaled and offset)
1715  f3x3matrixAeqScalar(pthisMagCal->fA, 0.0F);
1716  det = 1.0F;
1717  for (l = CHX; l <= CHZ; l++)
1718  {
1719  pthisMagCal->fA[l][l] = pthisMagCal->fmatB[l][j];
1720  det *= pthisMagCal->fA[l][l];
1721  pthisMagCal->ftrV[l] = -0.5F *
1722  pthisMagCal->fmatB[l + 3][j] /
1723  pthisMagCal->fA[l][l];
1724  }
1725 
1726  // negate A if it has negative determinant
1727  if (det < 0.0F)
1728  {
1729  f3x3matrixAeqMinusA(pthisMagCal->fA);
1730  pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
1731  det = -det;
1732  }
1733 
1734  // set ftmp to the square of the trial geomagnetic field strength B (counts times FMATRIXSCALING)
1735  ftmp = -pthisMagCal->fmatB[6][j];
1736  for (l = CHX; l <= CHZ; l++)
1737  {
1738  ftmp += pthisMagCal->fA[l][l] *
1739  pthisMagCal->ftrV[l] *
1740  pthisMagCal->ftrV[l];
1741  }
1742 
1743  // normalize the ellipsoid matrix A to unit determinant
1744  f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
1745 
1746  // convert the geomagnetic field strength B into uT for normalized soft iron matrix A and normalize
1747  pthisMagCal->ftrB = sqrtf(fabsf(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
1748 
1749  // compute trial invW from the square root of A also with normalized determinant and hard iron offset in uT
1750  f3x3matrixAeqI(pthisMagCal->ftrinvW);
1751  for (l = CHX; l <= CHZ; l++)
1752  {
1753  pthisMagCal->ftrinvW[l][l] = sqrtf(fabsf(pthisMagCal->fA[l][l]));
1754  pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] *
1755  DEFAULTB +
1756  (float) iOffset[l] *
1757  pthisMag->fuTPerCount;
1758  }
1759 
1760  return;
1761 }
1762 
1763 // 10 element calibration using direct eigen-decomposition
1764 void fComputeMagCalibration10(MagCalibration *pthisMagCal,
1765  MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
1766 {
1767  // local variables
1768  float det; // matrix determinant
1769  float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
1770  float ftmp; // scratch variable
1771  int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
1772  int16 iCount; // number of measurements counted
1773  int8 i,
1774  j,
1775  k,
1776  l,
1777  m,
1778  n; // loop counters
1779 
1780  // compute fscaling to reduce multiplications later
1781  fscaling = pthisMag->fuTPerCount / DEFAULTB;
1782 
1783  // the offsets are guaranteed to be set from the first element but to avoid compiler error
1784  iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0;
1785 
1786  // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
1787  for (m = 0; m < 10; m++)
1788  {
1789  for (n = m; n < 10; n++)
1790  {
1791  pthisMagCal->fmatA[m][n] = 0.0F;
1792  }
1793  }
1794 
1795  // add magnetic buffer entries into the 10x10 product matrix fmatA
1796  iCount = 0;
1797  for (j = 0; j < MAGBUFFSIZEX; j++)
1798  {
1799  for (k = 0; k < MAGBUFFSIZEY; k++)
1800  {
1801  if (pthisMagBuffer->index[j][k] != -1)
1802  {
1803  // use first valid magnetic buffer entry as estimate for offset to help solution (bit counts)
1804  if (iCount == 0)
1805  {
1806  for (l = CHX; l <= CHZ; l++)
1807  {
1808  iOffset[l] = pthisMagBuffer->iBs[l][j][k];
1809  }
1810  }
1811 
1812  // apply the fixed offset and scaling and enter into fvecA[6-8]
1813  for (l = CHX; l <= CHZ; l++)
1814  {
1815  pthisMagCal->fvecA[l + 6] = (float)
1816  (
1817  (int32) pthisMagBuffer->iBs[l][j][k] -
1818  (int32) iOffset[l]
1819  ) * fscaling;
1820  }
1821 
1822  // compute measurement vector elements fvecA[0-5] from fvecA[6-8]
1823  pthisMagCal->fvecA[0] = pthisMagCal->fvecA[6] * pthisMagCal->fvecA[6];
1824  pthisMagCal->fvecA[1] = 2.0F *
1825  pthisMagCal->fvecA[6] *
1826  pthisMagCal->fvecA[7];
1827  pthisMagCal->fvecA[2] = 2.0F *
1828  pthisMagCal->fvecA[6] *
1829  pthisMagCal->fvecA[8];
1830  pthisMagCal->fvecA[3] = pthisMagCal->fvecA[7] * pthisMagCal->fvecA[7];
1831  pthisMagCal->fvecA[4] = 2.0F *
1832  pthisMagCal->fvecA[7] *
1833  pthisMagCal->fvecA[8];
1834  pthisMagCal->fvecA[5] = pthisMagCal->fvecA[8] * pthisMagCal->fvecA[8];
1835 
1836  // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
1837  // with the exception of fmatA[9][9] which equals the number of measurements
1838  // update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9]
1839  for (m = 0; m < 9; m++)
1840  {
1841  pthisMagCal->fmatA[m][9] += pthisMagCal->fvecA[m];
1842  }
1843 
1844  // update the on and above diagonal terms of fmatA ignoring right hand column 9
1845  for (m = 0; m < 9; m++)
1846  {
1847  for (n = m; n < 9; n++)
1848  {
1849  pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
1850  }
1851  }
1852 
1853  // increment the measurement counter for the next iteration
1854  iCount++;
1855  }
1856  }
1857  }
1858 
1859  // set the last element fmatA[9][9] to the number of measurements
1860  pthisMagCal->fmatA[9][9] = (float) iCount;
1861 
1862  // store the number of measurements accumulated (defensive programming, should never be needed)
1863  pthisMagBuffer->iMagBufferCount = iCount;
1864 
1865  // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
1866  for (m = 1; m < 10; m++)
1867  {
1868  for (n = 0; n < m; n++)
1869  {
1870  pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
1871  }
1872  }
1873 
1874  // set pthisMagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
1875  fEigenCompute10(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB,
1876  10);
1877 
1878  // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
1879  j = 0;
1880  for (i = 1; i < 10; i++)
1881  {
1882  if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
1883  {
1884  j = i;
1885  }
1886  }
1887 
1888  pthisMagCal->fA[0][0] = pthisMagCal->fmatB[0][j];
1889  pthisMagCal->fA[0][1] = pthisMagCal->fA[1][0] = pthisMagCal->fmatB[1][j];
1890  pthisMagCal->fA[0][2] = pthisMagCal->fA[2][0] = pthisMagCal->fmatB[2][j];
1891  pthisMagCal->fA[1][1] = pthisMagCal->fmatB[3][j];
1892  pthisMagCal->fA[1][2] = pthisMagCal->fA[2][1] = pthisMagCal->fmatB[4][j];
1893  pthisMagCal->fA[2][2] = pthisMagCal->fmatB[5][j];
1894 
1895  // negate entire solution if A has negative determinant
1896  det = f3x3matrixDetA(pthisMagCal->fA);
1897  if (det < 0.0F)
1898  {
1899  f3x3matrixAeqMinusA(pthisMagCal->fA);
1900  pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
1901  pthisMagCal->fmatB[7][j] = -pthisMagCal->fmatB[7][j];
1902  pthisMagCal->fmatB[8][j] = -pthisMagCal->fmatB[8][j];
1903  pthisMagCal->fmatB[9][j] = -pthisMagCal->fmatB[9][j];
1904  det = -det;
1905  }
1906 
1907  // compute the inverse of the ellipsoid matrix
1908  f3x3matrixAeqInvSymB(pthisMagCal->finvA, pthisMagCal->fA);
1909 
1910  // compute the trial hard iron vector in offset bit counts times FMATRIXSCALING
1911  for (l = CHX; l <= CHZ; l++)
1912  {
1913  pthisMagCal->ftrV[l] = 0.0F;
1914  for (m = CHX; m <= CHZ; m++)
1915  {
1916  pthisMagCal->ftrV[l] += pthisMagCal->finvA[l][m] * pthisMagCal->fmatB[m + 6][j];
1917  }
1918 
1919  pthisMagCal->ftrV[l] *= -0.5F;
1920  }
1921 
1922  // compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING
1923  pthisMagCal->ftrB = sqrtf(fabsf(pthisMagCal->fA[0][0] *
1924  pthisMagCal->ftrV[CHX] * pthisMagCal->ftrV[CHX] +
1925  2.0F * pthisMagCal->fA[0][1] *
1926  pthisMagCal->ftrV[CHX] * pthisMagCal->ftrV[CHY] +
1927  2.0F * pthisMagCal->fA[0][2] *
1928  pthisMagCal->ftrV[CHX] * pthisMagCal->ftrV[CHZ] +
1929  pthisMagCal->fA[1][1] * pthisMagCal->ftrV[CHY] *
1930  pthisMagCal->ftrV[CHY] + 2.0F *
1931  pthisMagCal->fA[1][2] * pthisMagCal->ftrV[CHY] *
1932  pthisMagCal->ftrV[CHZ] + pthisMagCal->fA[2][2] *
1933  pthisMagCal->ftrV[CHZ] * pthisMagCal->ftrV[CHZ] -
1934  pthisMagCal->fmatB[9][j]));
1935 
1936  // calculate the trial normalized fit error as a percentage
1937  pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabsf(pthisMagCal->fvecA[j]) /
1938  (float) pthisMagBuffer->iMagBufferCount) / (pthisMagCal->ftrB * pthisMagCal->ftrB);
1939 
1940  // correct for the measurement matrix offset and scaling and get the computed hard iron offset in uT
1941  for (l = CHX; l <= CHZ; l++)
1942  {
1943  pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] *
1944  DEFAULTB +
1945  (float) iOffset[l] *
1946  pthisMag->fuTPerCount;
1947  }
1948 
1949  // convert the trial geomagnetic field strength B into uT for un-normalized soft iron matrix A
1950  pthisMagCal->ftrB *= DEFAULTB;
1951 
1952  // normalize the ellipsoid matrix A to unit determinant and correct B by root of this multiplicative factor
1953  f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
1954  pthisMagCal->ftrB *= powf(det, -(ONESIXTH));
1955 
1956  // compute trial invW from the square root of fA (both with normalized determinant)
1957  // set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
1958  // where fmatA holds the 3x3 matrix fA in its top left elements
1959  for (i = 0; i < 3; i++)
1960  {
1961  for (j = 0; j < 3; j++)
1962  {
1963  pthisMagCal->fmatA[i][j] = pthisMagCal->fA[i][j];
1964  }
1965  }
1966 
1967  fEigenCompute10(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB,
1968  3);
1969 
1970  // set pthisMagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
1971  for (j = 0; j < 3; j++) // loop over columns j
1972  {
1973  ftmp = sqrtf(sqrtf(fabsf(pthisMagCal->fvecA[j])));
1974  for (i = 0; i < 3; i++) // loop over rows i
1975  {
1976  pthisMagCal->fmatB[i][j] *= ftmp;
1977  }
1978  }
1979 
1980  // set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
1981  // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
1982  // loop over rows
1983  for (i = 0; i < 3; i++)
1984  {
1985  // loop over on and above diagonal columns
1986  for (j = i; j < 3; j++)
1987  {
1988  pthisMagCal->ftrinvW[i][j] = 0.0F;
1989 
1990  // accumulate the matrix product
1991  for (k = 0; k < 3; k++)
1992  {
1993  pthisMagCal->ftrinvW[i][j] += pthisMagCal->fmatB[i][k] * pthisMagCal->fmatB[j][k];
1994  }
1995 
1996  // copy to below diagonal element
1997  pthisMagCal->ftrinvW[j][i] = pthisMagCal->ftrinvW[i][j];
1998  }
1999  }
2000 
2001  return;
2002 }
2003 #endif
MagCalibration::ftrB
float ftrB
trial value of geomagnetic field magnitude in uT
Definition: magnetic.h:85
fmatrixAeqInvA
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ on exit...
Definition: matrix.c:666
MagCalibration::i7ElementSolverTried
int8_t i7ElementSolverTried
flag to denote at least one attempt made with 4 element calibration
Definition: magnetic.h:102
MAGBUFFSIZEX
#define MAGBUFFSIZEX
x dimension in magnetometer buffer (12x24 equals 288 elements)
Definition: magnetic.h:44
fComputeMagCalibration4
void fComputeMagCalibration4(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
Definition: magnetic.c:1406
CHY
#define CHY
Used to access Y-channel entries in various data data structures.
Definition: sensor_fusion.h:77
MagCalibration::fmatB
float fmatB[10][10]
scratch 10x10 float matrix used by calibration algorithms
Definition: magnetic.h:90
MagSensor::iBs
int16_t iBs[3]
averaged uncalibrated measurement (counts)
Definition: sensor_fusion.h:239
DEFAULTB
#define DEFAULTB
MINMEASUREMENTS7CAL
#define MINMEASUREMENTS7CAL
minimum number of measurements for 7 element calibration
Definition: magnetic.h:47
fInitializeMagCalibration
void fInitializeMagCalibration(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer)
Definition: magnetic.c:41
MagBuffer::tanarray
int16_t tanarray[MAGBUFFSIZEX-1]
array of tangents of (100 * angle)
Definition: magnetic.h:67
MagCalibration::iMeanBs
int32_t iMeanBs[3]
average magnetic measurement (counts)
Definition: magnetic.h:95
FITERRORAGINGSECS
#define FITERRORAGINGSECS
24 hours: time (s) for fit error to increase (age) by e=2.718
Definition: magnetic.h:53
f3x3matrixAeqMinusA
void f3x3matrixAeqMinusA(float A[][3])
function negates all elements of 3x3 matrix A
Definition: matrix.c:147
f3x3matrixAeqAxScalar
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
function multiplies all elements of 3x3 matrix A by the specified scalar
Definition: matrix.c:128
MagCalibration::fvecB
float fvecB[4]
scratch 4x1 vector used by calibration algorithms
Definition: magnetic.h:92
iUpdateMagBuffer
void iUpdateMagBuffer(MagBuffer *pthisMagBuffer, MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:103
MagCalibration::iNewCalibrationAvailable
int8_t iNewCalibrationAvailable
flag denoting that a new calibration has been computed
Definition: magnetic.h:98
MagCalibration::iInitiateMagCal
int8_t iInitiateMagCal
flag to start a new magnetic calibration
Definition: magnetic.h:99
fInvertMagCal
void fInvertMagCal(MagSensor *pthisMag, MagCalibration *pthisMagCal)
Definition: magnetic.c:297
MagBuffer::iMagBufferCount
int16_t iMagBufferCount
number of magnetometer readings
Definition: magnetic.h:68
f3x3matrixDetA
float f3x3matrixDetA(float A[][3])
function calculates the determinant of a 3x3 matrix
Definition: matrix.c:209
PI
#define PI
pi
Definition: sensor_fusion.h:97
int32
int32_t int32
Definition: sensor_fusion.h:57
MagCalibration::ftrV
float ftrV[3]
trial value of hard iron offset z, y, z (uT)
Definition: magnetic.h:83
MagCalibration
Magnetic Calibration Structure.
Definition: magnetic.h:72
uint32
uint32_t uint32
Definition: sensor_fusion.h:60
MagCalibration::ftrinvW
float ftrinvW[3][3]
trial inverse soft iron matrix size
Definition: magnetic.h:84
MagSensor::fBc
float fBc[3]
averaged calibrated measurement (uT)
Definition: sensor_fusion.h:236
MAXMEASUREMENTS
#define MAXMEASUREMENTS
maximum number of measurements used for calibration
Definition: magnetic.h:49
fComputeMagCalibration7
void fComputeMagCalibration7(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
Definition: magnetic.c:1601
MagCalibration::iSumBs
int32_t iSumBs[3]
sum of measurements in buffer (counts)
Definition: magnetic.h:94
MagCalibration::i4ElementSolverTried
int8_t i4ElementSolverTried
flag to denote at least one attempt made with 4 element calibration
Definition: magnetic.h:101
MagCalibration::iValidMagCal
int32_t iValidMagCal
solver used: 0 (no calibration) or 4, 7, 10 element
Definition: magnetic.h:80
MagCalibration::fmatA
float fmatA[10][10]
scratch 10x10 float matrix used by calibration algorithms
Definition: magnetic.h:89
MagCalibration::fvecA
float fvecA[10]
scratch 10x1 vector used by calibration algorithms
Definition: magnetic.h:91
MagCalibration::ftrFitErrorpc
float ftrFitErrorpc
trial value of fit error %
Definition: magnetic.h:86
fEigenCompute10
void fEigenCompute10(float A[][10], float eigval[], float eigvec[][10], int8 n)
function computes all eigenvalues and eigenvectors of a real symmetric matrix A[0..n-1][0..n-1] stored in the top left of a 10x10 array A[10][10]
Definition: matrix.c:234
fUpdateMagCalibration7Slice
void fUpdateMagCalibration7Slice(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
Definition: magnetic.c:686
MagCalibration::finvW
float finvW[3][3]
current inverse soft iron matrix
Definition: magnetic.h:76
MagCalibration::iMagBufferReadOnly
int8_t iMagBufferReadOnly
flag to denote that the magnetic measurement buffer is temporarily read only
Definition: magnetic.h:100
fComputeMagCalibration10
void fComputeMagCalibration10(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
Definition: magnetic.c:1764
MagCalibration::fV
float fV[3]
current hard iron offset x, y, z, (uT)
Definition: magnetic.h:75
MagSensor::fuTPerCount
float fuTPerCount
uT per count
Definition: sensor_fusion.h:237
ONESIXTH
#define ONESIXTH
one sixth
Definition: sensor_fusion.h:103
MINMEASUREMENTS10CAL
#define MINMEASUREMENTS10CAL
minimum number of measurements for 10 element calibration
Definition: magnetic.h:48
MATRIX_7_SIZE
#define MATRIX_7_SIZE
sensor_fusion.h
The sensor_fusion.h file implements the top level programming interface.
CAL_INTERVAL_SECS
#define CAL_INTERVAL_SECS
300s or 5min interval for regular calibration checks
Definition: magnetic.h:50
ONETHIRD
#define ONETHIRD
one third
Definition: sensor_fusion.h:102
CHZ
#define CHZ
MagCalibration::fA
float fA[3][3]
ellipsoid matrix A
Definition: magnetic.h:87
fRunMagCalibration
void fRunMagCalibration(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer, MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:325
f3x3matrixAeqInvSymB
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
function directly calculates the symmetric inverse of a symmetric 3x3 matrix only the on and above di...
Definition: matrix.c:168
fUpdateMagCalibration10Slice
void fUpdateMagCalibration10Slice(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
Definition: magnetic.c:973
fComputeEigSlice
void fComputeEigSlice(float fmatA[10][10], float fmatB[10][10], float fvecA[10], int8 i, int8 j, int8 iMatrixSize)
Definition: matrix.c:572
fUpdateMagCalibration4Slice
void fUpdateMagCalibration4Slice(MagCalibration *pthisMagCal, MagBuffer *pthisMagBuffer, MagSensor *pthisMag)
Definition: magnetic.c:455
MagSensor::iBc
int16_t iBc[3]
averaged calibrated measurement (counts)
Definition: sensor_fusion.h:240
CHX
#define CHX
Used to access X-channel entries in various data data structures.
Definition: sensor_fusion.h:76
MagCalibration::fFitErrorpc
float fFitErrorpc
current fit error %
Definition: magnetic.h:79
f3x3matrixAeqI
void f3x3matrixAeqI(float A[][3])
function sets the 3x3 matrix A to the identity matrix
Definition: matrix.c:45
MagSensor::fCountsPeruT
float fCountsPeruT
counts per uT
Definition: sensor_fusion.h:238
MESHDELTACOUNTS
#define MESHDELTACOUNTS
magnetic buffer mesh spacing in counts (here 5uT)
Definition: magnetic.h:54
MATRIX_10_SIZE
#define MATRIX_10_SIZE
MagCalibration::iCalInProgress
int8_t iCalInProgress
flag denoting that a calibration is in progress
Definition: magnetic.h:97
MagCalibration::finvA
float finvA[3][3]
inverse of ellipsoid matrix A
Definition: magnetic.h:88
MagSensor::fBs
float fBs[3]
averaged un-calibrated measurement (uT)
Definition: sensor_fusion.h:235
MagCalibration::fYTY
float fYTY
Y^T.Y for 4 element calibration = (iB^2)^2.
Definition: magnetic.h:93
FUSION_HZ
#define FUSION_HZ
(int) actual rate of fusion algorithm execution and sensor FIFO reads
Definition: standard_build.h:85
MagBuffer::index
int32_t index[MAGBUFFSIZEX][MAGBUFFSIZEY]
array of time indices
Definition: magnetic.h:66
MagSensor
The MagSensor structure stores raw and processed measurements for a 3-axis magnetic sensor...
Definition: sensor_fusion.h:227
MAXBFITUT
#define MAXBFITUT
maximum acceptable geomagnetic field B (uT) for valid calibration
Definition: magnetic.h:52
MINMEASUREMENTS4CAL
#define MINMEASUREMENTS4CAL
minimum number of measurements for 4 element calibration
Definition: magnetic.h:46
f3x3matrixAeqScalar
void f3x3matrixAeqScalar(float A[][3], float Scalar)
function sets every entry in the 3x3 matrix A to a constant scalar
Definition: matrix.c:109
MagCalibration::fB
float fB
current geomagnetic field magnitude (uT)
Definition: magnetic.h:77
int16
int16_t int16
Definition: sensor_fusion.h:56
MagBuffer
The Magnetometer Measurement Buffer holds a 3-dimensional "constellation" of data points...
Definition: magnetic.h:63
MagCalibration::fBSq
float fBSq
square of fB (uT^2)
Definition: magnetic.h:78
MAGBUFFSIZEY
#define MAGBUFFSIZEY
y dimension in magnetometer buffer (12x24 equals 288 elements)
Definition: magnetic.h:45
int8
int8_t int8
Definition: sensor_fusion.h:55
MINBFITUT
#define MINBFITUT
minimum acceptable geomagnetic field B (uT) for valid calibration
Definition: magnetic.h:51
MagBuffer::iBs
int16_t iBs[3][MAGBUFFSIZEX][MAGBUFFSIZEY]
uncalibrated magnetometer readings
Definition: magnetic.h:65
MagCalibration::itimeslice
int32_t itimeslice
counter for tine slicing magnetic calibration calculations
Definition: magnetic.h:96
MagCalibration::i10ElementSolverTried
int8_t i10ElementSolverTried
flag to denote at least one attempt made with 4 element calibration
Definition: magnetic.h:103